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1 . _ .Abstract 


Collision-free trajectory planning for robotic manipulators Is Investigated,^ th is paper. 
The task of the manipulator Is to move Its end-effector from one point to another point In an 
environment with polyhedral obstacles. An on-line algorithm Is developed based on finding the 
required Joint angles of the manipulator, according to goals with different priorities. The 
highest priority Is to avoid collisions, the second priority Is to plan the shortest path for 
the end effector, and the lowest priority Is to minimize the Joint velocity for smooth motion. 
The pseudo- Inverse of the Jacobian matrix Is applied for Inverse kinematics. When a possible 
collision Is detected, a constrained Inverse kinematic problem Is solved such that the collision 
Is avoided. This algorithm can also be applied to a time-variant environment. 

2. Introduction 

Ordinary tasks for a robotic manipulator are to move Its end effector from an admissible 
point to another admissible point In an environment with obstacles. For that, the Initial and 
final configuration of the manipulator are often given for the trajectory planning. Usually, 
there are Infinite paths for the end effector. Even for a specific path of the end effector, 
there are still Infinite trajectories possible for the manipulators. However, some of the 
trajectories are not feasible becasue of arm geometry, obstacles, and some kinematic or dynamic 
constraints. Even with the kinematically feasible trajectories, some computational or logic 
problems In the algorithms may make them Impractical. 

3. Trajectory Planning 


In order to move from one point to another. In the task space, one needs to solve for the 
angular Information from the spatial Information, using the tnverse kinematic relationship. 
Consider a robotic manipulator with n degrees of freedom. Let the kinematic relationship 
between Joint angles and the end-effector position and orientation be given by 

X - f(q) (1) 

where X Is the m-dlmentlonal vector of the end-effector position and orientation, and q Is the 
n-dlmenslonal vector of Joint angles. For a kinematically redundant manipulator, the dimension 
of q Is greater than the dimension of X, (n>m). Differentiating the above relation, we get 

X - J(q)q (2) 

where J(q) * df/dq Is the mxn Jacobian matrix, [7]. For a redundant manipulator, the Jacobian 
matrix will have more columns than rows. Moreover, the Inverse of such non-square matrix Is not 
defined In a regular sense. However, useful solutions of equation (2) can be found, by using 
the generalized-inverses of the Jacobian matrix J, and Is given by 

q - J f X • (I-J f J)Z (3) 

t T T -1 

where J - J (JJ ) Is the pseudo- Inverse of J, I Is the nxn Identity matrix, and 2 is an 
arbitrary n-dimensional vector. When the vector 7. is selected to be zero, equation (3) reduces 
to 

q - J f X (*) 

which gives the best approximate solution to equation (2). This Is In the sense that If q. Is 

the solution or (2), given by (k), then ||q.|| < ||q||. where q Is any other solution of (2) 
that Is given by (3), [5-6]. It should be noted that, such minimum norm, or best approximate 

solution. 13 defined when there Is no restriction In the task space. This means that, In a 
restricted environment, the above mentioned best approximate, solution may not be feasible for 
application and may result in collision. 
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.INIENVMMttU 


Let ua define collision point to be the point on the nanlpulator body which has the 
potential to collide with the obstacle. The collision-free trajectory planning problem here Is 

to develop an algorithm, for the on-line determination of the required Joint angle rates, q, for 
safe manipulator motion. The approach is to continuously monitor the task space, for detecting 
possible collisions. If no potential collision Is detected, the required Joint angle rates are 
generated, using the best approximate solution, to move the end-effector on a shortest distance. 
But when a potential collision point is detected, the trajectory is modified In order to avoid 
collision. 

*». Obstacle Avoidance 


In order to avoid obstacles, one needs to use the kinematic relationship for the oolllslon 
points, similar to that of equations (1) and (2). Let the potential collision point, In the 
task space, be denoted by X fl . Then, similar to equation (2), we can write 

X c - J 0 (q)q (5) 

where J is the mxn Jacobian matrix for the collision point. The Inverse kinematic solution to 
the above Is similar to (3) and Is given by 


q - J r x ♦ (I-j!j )Z' 
c. c c c 

where J* Is the pseudo- Inverse of J., and Z' 

c - c 


Is an arbitrary n-dlmenslonal vector. 


( 6 ) 


Now, the problem of obstacle avoidance is that, when a potential collision is detected, the 
highest priority Is to avoid the obstacle, and, If needed modify the position of end-effector. 

In order for the trajectory planning to have minimum norm, we choose Z - 0 as In (k). On 

the other hand we choose Z'«0, like in (6), to account for both collision avoidance and 
trajectory planning. From (3) and (65, a minimum norm solution for V Is 


Z’ - [J(I-J*J J^CX-JJ^X 1. 

G C CO 

Plugging this back Into (6), we get 


J*X 
c c 


♦ (i-J*j JCJU-j'jJ^Cx-JjV,]. 

C C C C C C 


Then, using the following Identity, [6] 


(I-j’j JtJlI-/)] 1 - [JtW^JJ] 1 
c c c c c 

we get 

q - J*X„ * [J(I-J*J J^tX-JjV]. 
c c c c c c 


( 7 ) 


The above relation, generates the Joint angle rates q such that the obstacle is avoided and the 
end effector velocity Is modified, for the on-line trajectory planning. 

Now the question Is how and In what direction the end effector spatial velocity should be 
changed. For the algorithm to be fast and Implementable, a finite search for the minimum norm 

solution is considered. The value of X c l3 preselected by the user, and the value of X 

modification is also preselected by a value of c. The value of q may now be found by examining 
seven different directions for rate modification, e.g., e-varlatlon In plus or minus X.t.Z 

coordinates and also no modification. The smallest norm ||q|| Is then chosen for trajectory 
modification, from seven different possibilities. 

The overall algorithm Is such that when no potential collision is detected, a minimum norm 

solution q Is planned according to (4), But, when a potential collision Is detected, the 
obstacle Is avoided and the path In modified according to (7). 

In order to develop the algorithm. It Is assumed that: 

(1) the solution exists 

(2) the obstacles are represented by polyhedrals. 

(3) the geometrical Information about the task area Is known, e.g., using sensory systems, the 
positions of obstacles and manipulator links are known. 

(H) the potential collision point on the manipulator links can be detected. 

(5) only a single collision may occur, and will be detected, at any given time. 
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5. Th« Algorithm 


The following summarizes the steps, Involved In the proposed slgorltha, for the on-line 

eolllslon-free trajectory planning of the robotlo manipulators. The steps of the algorithm are: 

(1) Determine a minimum-length path for the end-effector, froe the current position to target 
position. 

(2) Check if there Is a potential collision point. If there la, go to step (5), otherwise 
continue. 

(3) No potential collision Is detected. Make an incremental aove according to the Joint angle 
rates vector q, given by equation (4). 

(1) If the end-effector has not reached the target, go to step (2). If it has reached the 
target, go to step (7). 

(5) Potential collision Is detected. Make an Incremental move according to the Joint angle 
rates vector q, given by equation (7), such that ||q|| Is minimized In a finite search. 

(6) Co to step (1 ). 

(7) Stop. 

6. Conclusion 


On-line, collision-free trajectory planning Is discussed. An algorithm, which utilizes 
sensed information about the configuration of the manipulator and obstacles, Is developed based 
on the task priorities. The order of the task priorities are: to avoid collision, to plan the 

shortest path for the end effector, and to choose the minimum norm solution. The algorithm Is 
fast and could be Implemented on robotic manipulators for on-line trajectory planning. 
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